/*
 * mox_master_c.c
 *
 *  Created on: Jul 11, 2010
 *      Author: jmonk
 */
#include <xs1.h>
#include <stdlib.h>
#include <print.h>
#include <stdio.h>
#include <gcc/stdarg.h>
#include "mox.h"
#include "mox_storage.h"
#include "mox_module.h"
#include "mox_log.h"
#include "mox_threads.h"
#include "module_flash.h"
#ifdef FLASHER
#include "mox_flasher.h"
#endif

void module_start_uart_0(void);

struct module_at_start module_flash = {
		0x1000,
		(void*)module_start_flash,
		NULL,
		NULL
};

struct module_at_start module_uart = {
		0x1000,
		(void*)NULL,
		NULL,
		NULL
};

void mox_master(chanend chan_mox[NUM_CORES])
{
	mox_master_state state;
	chanend temp_chan;
	int i;
	void *p;
	mox_msg *msg;

	for (i = 0; i < NUM_CORES; i++) state.chan_mox[i] = chan_mox[i];

	init_master(&state, chan_mox[NUM_CORES], LOGGER_CORE);

#ifdef FLASHER

	mox_clear_chans(&state);
	flasher_master(&state);

	while (1) {
		master_log(&state, "Master Main Loop Iteration\n");
		mox_clear_chans(&state);
		mox_service_slaves((unsigned)&state, chan_mox, state.frame);
		delay_us(1000000);
	}
#else
	//mox_clear_chans(&state);
	//load_module(&state,"UARTMOD");
	//p = malloc(512);
	mox_clear_chans(&state);
	mox_service_slaves((unsigned)&state, chan_mox, state.frame);

//	init_module_start(&state, 0, &module_uart, "UARTMOD");

	mox_clear_chans(&state);
	mox_service_slaves((unsigned)&state, chan_mox, state.frame);
	mox_clear_chans(&state);

	msg = (mox_msg *)malloc(sizeof(mox_msg));
	if (msg) {
		msg->type = TH_PARAM;
		msg->size = 0;
		msg->content = NULL;
		if (mox_req_com(state.chan_mox[state.thd_core[6]], 6)) {
			mox_comm_msg_send_thd(state.chan_mox[state.thd_core[6]], *msg);
			mox_end_com(state.chan_mox[state.thd_core[6]]);
		}
		free(msg);
	}

	mox_clear_chans(&state);
	mox_service_slaves((unsigned)&state, chan_mox, state.frame);
	mox_clear_chans(&state);

	/*
	msg = (mox_msg *)malloc(sizeof(mox_msg));
	if (msg) {
		msg->type = TH_PARAM;
		msg->size = 0;
		msg->content = NULL;
		if (mox_req_com(state.chan_mox[state.thd_core[6]], 6)) {
			mox_comm_msg_send_thd(state.chan_mox[state.thd_core[6]], *msg);
			mox_end_com(state.chan_mox[state.thd_core[6]]);
		}
		free(msg);
	}*/

	master_log(&state, "Entering Master Main Loop\n");
	while (1) {
		mox_clear_chans(&state);
		mox_service_slaves((unsigned)&state, chan_mox, state.frame);
		delay_us(1000000);
		printstrln("Main Loop Iteration");
	}
#endif

	return;
}

#pragma stackfunction 20
void *mox_send_cmd(mox_master_state *state, mox_cmd_t cmd, int core_n, int core_n2)
{
	mox_msg *msg;
	void *p;
	mox_msg_t resp;

	msg = (mox_msg *)malloc(sizeof(mox_msg));
	if (!msg) return NULL;
	msg->type = MOX_CMD;
	switch (cmd) {
	case LOG_UPDATE:
		//chan_out(state->chan_mox[core_n], MOX_NACK);
		//chan_out(state->chan_mox[core_n], MOX_NACK);

		// Get Channels
		if (state->log->core_id == core_n) {
			p = mox_send_cmd(state, CHAN_CREATE_LOC, core_n, 0);
		} else {
			p = mox_send_cmd(state, CHAN_CREATE_REM, core_n, state->log->core_id);
		}
		if (mox_log_req_add(state, ((chanend *)p)[1])) {
			free(msg);
			return NULL;
		}

		// Compose Message
		msg->content = (unsigned char *)malloc(3 * sizeof(unsigned));
		if (!msg->content) {
			free(msg);
			return NULL;
		}
		msg->size = 3 * sizeof(unsigned);
		((unsigned *)msg->content)[0] = LOG_UPDATE;
		((chanend *)msg->content)[1] = ((chanend *)p)[0];
		((unsigned *)msg->content)[2] = state->log->core_id;

		// Send Message
		mox_outct(state->chan_mox[core_n],MOX_MSG);
		mox_comm_msg_send(state->chan_mox[core_n], *msg);

		// Free Memory
		free(msg->content);
		free(p);
		break;
	case CHAN_CREATE_LOC:
		p = malloc(sizeof(chanend)*2);

		msg->content = (unsigned char *)malloc(sizeof(unsigned));
		if (!msg->content) {
			free(msg);
			return NULL;
		}
		msg->size = sizeof(unsigned);

		((unsigned *)msg->content)[0] = CHAN_CREATE_LOC;

		// Send Message
		mox_outct(state->chan_mox[core_n],MOX_MSG);
		mox_comm_msg_send(state->chan_mox[core_n], *msg);

		free(msg->content);
		free(msg);

		msg = mox_comm_msg_recv(state->chan_mox[core_n]);
		if (!msg) return NULL;

		if ((msg->type == MOX_ACK) && msg->content && (((unsigned *)msg->content)[0] == CHAN_CREATE_LOC)) {
			((chanend *)p)[0] = ((chanend *)msg->content)[1];
			((chanend *)p)[1] = ((chanend *)msg->content)[2];
		} else {
			p = NULL;
		}

		return p;
	case CHAN_CREATE_REM:
		p = malloc(sizeof(chanend)*2);

		msg->content = (unsigned char *)malloc(2 * sizeof(unsigned));
		if (!msg->content) {
			free(msg);
			return NULL;
		}
		msg->size = 2 * sizeof(unsigned);

		((unsigned *)msg->content)[0] = CHAN_CREATE_REM;
		((unsigned *)msg->content)[1] = 1;

		mox_outct(state->chan_mox[core_n],MOX_MSG);
		mox_comm_msg_send(state->chan_mox[core_n], *msg);
		mox_outct(state->chan_mox[core_n],MOX_MSG);
		mox_comm_msg_send(state->chan_mox[core_n2], *msg);

		free(msg->content);
		free(msg);

		msg = mox_comm_msg_recv(state->chan_mox[core_n]);
		mox_outct(state->chan_mox[core_n],MOX_MSG);
		mox_comm_msg_send(state->chan_mox[core_n2], *msg);
		if (!msg) return NULL;

		if ((msg->type == MOX_ACK) && msg->content && (((unsigned *)msg->content)[0] == CHAN_CREATE_REM)) {
			((chanend *)p)[0] = ((chanend *)msg->content)[1];
		}

		free(msg->content);
		free(msg);

		msg = mox_comm_msg_recv(state->chan_mox[core_n2]);
		mox_outct(state->chan_mox[core_n],MOX_MSG);
		mox_comm_msg_send(state->chan_mox[core_n], *msg);
		if (!msg) return NULL;

		if ((msg->type == MOX_ACK) && msg->content && (((unsigned *)msg->content)[0] == CHAN_CREATE_REM)) {
			((chanend *)p)[1] = ((chanend *)msg->content)[1];
		}

		return p;
	default:
		//chan_out(state->chan_mox[core_n], MOX_NACK);
		//chan_out(state->chan_mox[core_n], MOX_NACK);
		//if (core_n2) {
		//	chan_out(state->chan_mox[core_n2], MOX_NACK);
		//	chan_out(state->chan_mox[core_n2], MOX_NACK);
		//}
		break;
	}

	return NULL;
}

int mox_log_req_add(mox_master_state *state, chanend add)
{
	mox_msg_t resp;
	mox_chan_out_wd(state->log->channel, LOG_REQ_ADD);
	resp = mox_chan_in_wd(state->log->channel);
	if (resp == LOG_ACK) {
		mox_chan_out_wd(state->log->channel, add);
		return 0;
	} else {
		return 1;
	}
}

void mox_slave_service(unsigned st, int core_num)
{
	mox_master_state *state = (mox_master_state *)st;
	unsigned thread_c_id;
	unsigned thread_id;
	unsigned temp, i;
	chanend temp_chan;
	chanend *chans;
	char shortname[8];
	mox_msg *msg = mox_comm_msg_recv(state->chan_mox[core_num]);

	if (!msg) return;
	state->slave_unrespond[core_num] = 0;
	switch (msg->type) {
	case TH_REQ_COM:
		break;
	case TH_START_MOD:
	case TH_START_APP:
		thread_id = ((unsigned *)msg->content)[0];
		((int *)shortname)[0] = ((unsigned *)msg->content)[1];
		((int *)shortname)[1] = ((unsigned *)msg->content)[2];

		temp = state->thd_cnt;
		load_module(state,shortname);

		if (state->thd_cnt - temp) {
			if (mox_req_com(state->chan_mox[state->thd_core[thread_id]], thread_id)) {
				free(msg->content);
				msg->content = malloc(2 * sizeof(unsigned));
				msg->type = TH_PARAM;
				msg->size = 2 * sizeof(unsigned);
				if (msg->content) {
					((unsigned *)msg->content)[0] = state->thd_cnt - temp;
					((unsigned *)msg->content)[1] = state->thd_cnt - 1;
				}
				mox_comm_msg_send_thd(state->chan_mox[state->thd_core[thread_id]], *msg);
				mox_end_com(state->chan_mox[state->thd_core[thread_id]]);
			}
		}

		break;
	case TH_REQ_CFG:
		thread_id = ((unsigned *)msg->content)[0];

		free(msg->content);
		msg->content = sizeof(mox_cfg_state);
		msg->size = sizeof(mox_cfg_state);
		msg->type = TH_PARAM;

		mox_comm_msg_send(state->chan_mox[state->thd_core[thread_id]], *msg);
		break;
	case TH_SET_CFG:
		thread_id = ((unsigned *)msg->content)[0];

		temp = sizeof(struct mox_cfg_state);
		while (temp & 0x03) temp++;

		for (i = 0; i < (temp / 4); i++) {
			((int *)state->cfg)[i] = ((int *)msg->content)[i + 1];
		}
		free(msg->content);
		msg->content = NULL;
		msg->size = 0;
		msg->type = TH_ACK;

		if (mox_req_com(state->chan_mox[state->thd_core[thread_id]], thread_id)) {
			mox_comm_msg_send_thd(state->chan_mox[state->thd_core[thread_id]], *msg);
			mox_end_com(state->chan_mox[state->thd_core[thread_id]]);
		}
		break;
	case TH_CON_REQ:
		thread_c_id = ((unsigned *)msg->content)[0];
		thread_id = ((unsigned *)msg->content)[1];
		temp = ((unsigned *)msg->content)[2];

		if (state->thd_core[thread_c_id] != state->thd_core[thread_id]) {
			chans = mox_send_cmd(&state, CHAN_CREATE_REM, state->thd_core[thread_c_id], state->thd_core[thread_id]);
		} else {
			chans = mox_send_cmd(&state, CHAN_CREATE_LOC, state->thd_core[thread_c_id], 0);
		}

		free(msg->content);
		msg->content = malloc(sizeof(chanend));
		if (msg->content) {
			msg->size = sizeof(chanend);
		} else {
			msg->size = 0;
		}
		((chanend *)msg->content)[0] = chans[0];

		if (temp) {
			msg->type = SIG_OCONN;
		} else {
			msg->type = SIG_ICONN;
		}

		if (mox_req_com(state->chan_mox[state->thd_core[thread_c_id]], thread_c_id)) {
			mox_comm_msg_send_thd(state->chan_mox[state->thd_core[thread_c_id]], *msg);
			free(msg);
			msg = mox_comm_msg_recv_thd(state->chan_mox[state->thd_core[thread_c_id]]);
			mox_end_com(state->chan_mox[state->thd_core[thread_c_id]]);

			if (!(msg->type == SIG_ACK) || !msg->content) {
				chans[1] = 0;
			}
			if (msg->content) {
				((chanend *)msg->content)[0] = chans[1];
			}
			msg->type = TH_PARAM;
			if (mox_req_com(state->chan_mox[state->thd_core[thread_id]], thread_id)) {
				mox_comm_msg_send_thd(state->chan_mox[state->thd_core[thread_id]], *msg);
				mox_end_com(state->chan_mox[state->thd_core[thread_id]]);
			}
		} else {
			if (msg->content) {
				free(msg->content);
				msg->content = NULL;
				msg->size = 0;
			}
		}
		break;
	case TH_DIS_REQ:
		thread_id = ((unsigned *)msg->content)[0];
		thread_c_id = ((unsigned *)msg->content)[1];
		temp = ((unsigned *)msg->content)[2];

		free(msg->content);
		msg->content = malloc(sizeof(unsigned));
		msg->size = 0;
		if (msg->content) {
			((unsigned *)msg->content)[0] = thread_id;
			msg->size = sizeof(unsigned);
		}
		if (temp) {
			msg->type = SIG_ODISC;
		} else {
			msg->type = SIG_IDISC;
			//mox_chan_out_wd(temp_chan, SIG_IDISC);
		}

		if (mox_req_com(state->chan_mox[state->thd_core[thread_c_id]], thread_c_id)) {
			mox_comm_msg_send_thd(state->chan_mox[state->thd_core[thread_c_id]], *msg);
			mox_end_com(state->chan_mox[state->thd_core[thread_c_id]]);
		}
		break;
	case TH_LST_REQ:
		thread_id = ((unsigned *)msg->content)[0];

		free(msg->content);
		msg->content = state->thd_core;
		msg->size = state->thd_cnt;
		msg->type = TH_PARAM;

		if (mox_req_com(state->chan_mox[state->thd_core[thread_id]], thread_id)) {
			mox_comm_msg_send_thd(state->chan_mox[state->thd_core[thread_id]], *msg);
			mox_end_com(state->chan_mox[state->thd_core[thread_id]]);
		}

		break;
	case TH_DTL_REQ:
		thread_id = ((unsigned *)msg->content)[0];
		temp = ((unsigned *)msg->content)[1];

		free(msg->content);
		msg->type = TH_PARAM;
		if (mox_req_com(state->chan_mox[state->thd_core[thread_id]], thread_id)) {
			msg->size = sizeof(struct mox_rthd);
			msg->content = &state->running_thds[temp];
			mox_comm_msg_send_thd(state->chan_mox[state->thd_core[thread_id]], *msg);

			msg->size = sizeof(struct mox_smod);
			msg->content = state->running_thds[temp].thd_info;
			mox_comm_msg_send_thd(state->chan_mox[state->thd_core[thread_id]], *msg);

			mox_end_com(state->chan_mox[state->thd_core[thread_id]]);
		}
		break;
	default:
		break;
	}

	return;
}

void mox_slave_no_respond(unsigned st, int core_num)
{
	mox_master_state *state = (mox_master_state *)st;
	state->slave_unrespond[core_num]++;
	master_log(&state, "Warning: Slave was unresponsive %d\n", core_num);

	return;
}

void init_master(mox_master_state *state, chanend log, chanend log_core)
{
	int i;
	int resp;
	void *func;
	int cfg_size;

	state->log = NULL;

	//thread array initialization
	for (i = 0; i < MAX_THDS; i++) {
		state->thd_core[i] = -1;
	}
	state->thd_cnt = 0;

	//running threads array startup
	state->running_thds = (struct mox_rthd *) malloc((MAX_THDS+1) * sizeof(struct mox_rthd));
	//while (((int)state->running_thds)&0x03) ((void*)state->running_thds)++;
	state->running_thds[state->thd_cnt].thd_info = (struct mox_smod *)malloc(sizeof(struct mox_smod));
	state->running_thds[state->thd_cnt].thd_info->name[0] = 'M';
	state->running_thds[state->thd_cnt].thd_info->name[1] = 'A';
	state->running_thds[state->thd_cnt].thd_info->name[2] = 'S';
	state->running_thds[state->thd_cnt].thd_info->name[3] = 'T';
	state->running_thds[state->thd_cnt].thd_info->name[4] = 'E';
	state->running_thds[state->thd_cnt].thd_info->name[5] = 'R';
	state->running_thds[state->thd_cnt].thd_info->name[6] = '0' + MASTER_CORE;
	state->running_thds[state->thd_cnt].thd_info->name[7] = '\0';
	state->running_thds[state->thd_cnt].thd_info->size = 0;
	state->running_thds[state->thd_cnt].thd_info->stack[0] = 0;
	state->running_thds[state->thd_cnt].type = TH_TYPE_MOX;
	state->thd_core[state->thd_cnt++] = MASTER_CORE;

	for (i = 0; i < MAX_THDS; i++) {
		state->running_thds[i].thd_info = NULL;
		state->running_thds[i].type = 0;
	}

	master_log(state, "Completed Running Threads Initialization\n");

	//configuration initialization
	cfg_size = sizeof(struct mox_cfg_state);
	while (cfg_size&0x03) cfg_size++;
	state->cfg = (struct mox_cfg_state *) malloc(cfg_size);
	state->cfg->cfg = 0;

	master_log(state, "Completed Realtime Configuration Initialization\n");

	//slave enumeration
	for (i = 0; i < NUM_CORES; i++) {
		mox_chan_out_wd(state->chan_mox[i], i);
		resp = mox_chan_in_wd(state->chan_mox[i]);
		if (resp == MOX_ACK) {
			state->slave_unrespond[i] = 0;
			state->running_thds[state->thd_cnt].thd_info = (struct mox_smod *)malloc(sizeof(struct mox_smod));
			state->running_thds[state->thd_cnt].thd_info->name[0] = 'S';
			state->running_thds[state->thd_cnt].thd_info->name[1] = 'L';
			state->running_thds[state->thd_cnt].thd_info->name[2] = 'C';
			state->running_thds[state->thd_cnt].thd_info->name[3] = 'O';
			state->running_thds[state->thd_cnt].thd_info->name[4] = 'R';
			state->running_thds[state->thd_cnt].thd_info->name[5] = 'E';
			state->running_thds[state->thd_cnt].thd_info->name[6] = '0' + i;
			state->running_thds[state->thd_cnt].thd_info->name[7] = '\0';
			state->running_thds[state->thd_cnt].thd_info->size = 0;
			state->running_thds[state->thd_cnt].thd_info->stack[0] = 0;
			state->running_thds[state->thd_cnt].type = TH_TYPE_MOX;
			state->thd_core[state->thd_cnt++] = i;
		} else {
			master_log(state, "Warning: Slave %d failed to enumerate\n", i);
		}
	}

	master_log(state, "Completed Slave Enumration\n");

	//log initialization
	//state->log = (struct system_log *) malloc(sizeof(struct system_log));

	//state->log->channel = log;
	//state->log->core_id = log_core;
	//for (i = 0; i < NUM_CORES; i++) {
	//	mox_send_cmd(state, LOG_UPDATE, i, 0);
	//}

	master_log(state, "Completed Log Initialization\n");

#ifndef FLASHER
	//thread startup receive
	func = (void *)mox_chan_in_wd(state->chan_mox[THD_START_CORE]);
#endif

	//service all slave requests once
	mox_service_slaves((unsigned)state, state->chan_mox, state->frame);

	//primary storage module startup
	mox_clear_chans(state);
	state->storage = (struct mox_storage*)malloc(sizeof(struct mox_storage));
	if (state->storage) {
		state->storage->num = 0;
		state->storage->mod = (struct mox_module **)malloc(MAX_STORAGE*sizeof(struct mox_module *));
		state->storage->prog_addr = (int *)malloc(MAX_STORAGE*sizeof(int));
	}

	//start actual module
	master_log(state, "Status: Starting Initial Storage Module...");
	if (STORAGE_START != NULL) {
		init_module_start(state, STORAGE_CORE, &STORAGE_STRUCT, STORAGE_NAME);
	} else {
		master_log(state, "Warning: Master has no initial Storage Device\n\tThis will prevent the loading of more Modules\n");
	}
	master_log(state, "Done\n");

#ifndef FLASHER
	//initial thread startup
	master_log(state, "Starting Initial Thread...");
	mox_send_init_crth(state, func, THD_START_CORE, 0x600, THD_START_NAME);
	master_log(state, "Done\n");
#endif

	return;
}

int mox_send_init_crth(mox_master_state *state, void *func, int core_id, int stack, char *name)
{
	int response;
	unsigned i;
	mox_msg *msg = malloc(sizeof(mox_msg));
	mox_msg_t type = TH_TYPE_APP;

	if (!msg) return 1;

	msg->content = malloc(sizeof(2 * sizeof(int)));
	if (msg->content) {
		((void **)msg->content)[0] = func;
		((int *)msg->content)[1] = stack;
		msg->size = 2 * sizeof(int);
	} else {
		msg->size = 0;
	}
	msg->type = MOX_INIT_CRTH;
	mox_outct(state->chan_mox[core_id], MOX_MSG);
	mox_comm_msg_send(state->chan_mox[core_id], *msg);

	if (msg->content) free(msg->content);
	free(msg);

	msg = mox_comm_msg_recv(state->chan_mox[core_id]);

	if ((msg->type != TH_TYPE_MOD) || (!msg->content)) {
		if (msg->content) free(msg->content);
		free(msg);
		return 1;
	}

	response = ((unsigned *)msg->content)[0];
	if (response == MOD_STORAGE) {
		state->storage->prog_addr[state->storage->num] = (int)0x40000;
		state->storage->mod[state->storage->num] = (struct mox_module*) malloc(sizeof(struct mox_module));
		state->storage->mod[state->storage->num]->core_id = state->chan_mox[core_id];
		state->storage->mod[state->storage->num++]->mod_id = state->thd_cnt;//((unsigned *)msg->content)[1];
		type = TH_TYPE_MOD;
	} else if (response == MOD_GEN) {
		type = TH_TYPE_MOD;
	} else if (response != TH_TYPE_APP) {
		if (msg->content) free(msg->content);
		free(msg);
		return 1;
	}

	state->running_thds[state->thd_cnt].thd_info = (struct mox_smod *)malloc(sizeof(struct mox_smod));
	for (i = 0; i < 8; i++) {
		state->running_thds[state->thd_cnt].thd_info->name[i] = name[i];
	}
	state->running_thds[state->thd_cnt].thd_info->size = 0;
	state->running_thds[state->thd_cnt].thd_info->stack[core_id] = stack;
	state->running_thds[state->thd_cnt].type = type;

	state->thd_core[state->thd_cnt] = core_id;
	if (msg->content) {
		((unsigned *)msg->content)[0] = state->thd_cnt++;
		msg->size = sizeof(unsigned);
		msg->type = MOX_ACK;
		mox_outct(state->chan_mox[core_id], MOX_MSG);
		mox_comm_msg_send(state->chan_mox[core_id], *msg);
		free(msg->content);
	}
	free(msg);

	return 0;
}

